
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_qmi8610.c
  * @author     baiyang
  * @date       2023-2-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_qmi8610.h"

#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
#define QMI8610_BACKEND_SAMPLE_RATE   1000
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_qmi8610_start(sensor_imu_backend *backend);
static bool sensor_imu_qmi8610_update(sensor_imu_backend *backend);
static bool init(sensor_imu_qmi8610_t qmi8610);
static bool hardware_init(sensor_imu_qmi8610_t qmi8610);
static uint8_t _register_read(sensor_imu_qmi8610_t qmi8610, uint8_t reg);
static inline bool _block_read(sensor_imu_qmi8610_t qmi8610, uint8_t reg, uint8_t *buf, uint32_t size);
static inline bool _register_write(sensor_imu_qmi8610_t qmi8610, uint8_t reg, uint8_t val);
static bool check_whoami(sensor_imu_qmi8610_t qmi8610);
static void read_sample(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_imu_backend_ops qmi8610_ops = {.sensor_imu_backend_destructor = NULL,
                                                    .update = sensor_imu_qmi8610_update,
                                                    .accumulate = NULL,
                                                    .start = sensor_imu_qmi8610_start,
                                                    .get_output_banner = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void sensor_imu_qmi8610_ctor(sensor_imu_qmi8610_t qmi8610,
                                    gp_device_t dev,
                                    enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(qmi8610, 0, sizeof(struct sensor_imu_qmi8610));

    sensor_imu_backend_ctor(&qmi8610->backend, &qmi8610_ops, "qmi8610");

    qmi8610->_rotation = rotation;
    
    qmi8610->_dev = dev;
}

sensor_imu_backend * sensor_imu_qmi8610_probe(gp_device_t dev, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_imu_qmi8610_t sensor = (sensor_imu_qmi8610_t)rt_malloc(sizeof(struct sensor_imu_qmi8610));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_qmi8610_ctor(sensor, dev, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static void sensor_imu_qmi8610_start(sensor_imu_backend *backend)
{
    sensor_imu_qmi8610_t qmi8610 = (sensor_imu_qmi8610_t)backend;

    //fifo_reset(qmi8610);

    uint32_t period_usec = 1000000UL / QMI8610_BACKEND_SAMPLE_RATE;
    if (devmgr_get_bus_type(qmi8610->_dev) == BUS_TYPE_I2C) {
        period_usec *= 4;
    }

    if (!sensor_imu_register_accel(&qmi8610->_accel_instance, 1000000UL/period_usec, devmgr_get_bus_id_devtype(qmi8610->_dev, DEVTYPE_INS_QMI8610)) ||
        !sensor_imu_register_gyro(&qmi8610->_gyro_instance, 1000000UL/period_usec, devmgr_get_bus_id_devtype(qmi8610->_dev, DEVTYPE_INS_QMI8610))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(qmi8610->_gyro_instance, qmi8610->_rotation);
    sensor_imu_backend_set_accel_orientation(qmi8610->_accel_instance, qmi8610->_rotation);

    // setup callbacks
    devmgr_register_periodic_callback(qmi8610->_dev, period_usec, read_sample, qmi8610);
}

static bool sensor_imu_qmi8610_update(sensor_imu_backend *backend)
{
    sensor_imu_qmi8610_t qmi8610 = (sensor_imu_qmi8610_t)backend;

    sensor_imu_backend_update_accel(backend, qmi8610->_accel_instance);
    sensor_imu_backend_update_gyro(backend, qmi8610->_gyro_instance);
    return true;
}

static bool init(sensor_imu_qmi8610_t qmi8610)
{
    if (devmgr_get_bus_type(qmi8610->_dev) == BUS_TYPE_SPI) {
        devmgr_set_read_flag(qmi8610->_dev, 0x80);
    }

    return hardware_init(qmi8610);
}

static bool hardware_init(sensor_imu_qmi8610_t qmi8610)
{
    devmgr_take_bus(qmi8610->_dev);

    rt_thread_mdelay(10);

    // initially run the bus at low speed
    devmgr_set_speed(qmi8610->_dev, DEV_SPEED_LOW);

    if (!check_whoami(qmi8610)) {
        goto failed;
    }

    //四线SPI，使能内部1M晶振
    if (devmgr_get_bus_type(qmi8610->_dev) == BUS_TYPE_SPI
        && !_register_write(qmi8610, Qmi8610Register_Ctrl1, 0x00)) {
        goto failed;
    }

    //加速度计自检，±16g，1000Hz采样
    if (!_register_write(qmi8610, Qmi8610Register_Ctrl2, Qmi8610AccRange_16g | Qmi8610AccOdr_1000Hz)) {
        goto failed;
    }

    qmi8610->_accel_scale = GRAVITY_MSS * 16.0f /32768.0f;

    //陀螺仪自检，±2048dps，1000Hz采样
    if (!_register_write(qmi8610, Qmi8610Register_Ctrl3, Qmi8610GyrRange_2048dps | Qmi8610GyrOdr_1000Hz)) {
        goto failed;
    }

    qmi8610->_gyro_scale = radians(2048.0f)/32768.0f;

    //使能陀螺仪、加速度计低通滤波,截止频率200Hz
    if (!_register_write(qmi8610, Qmi8610Register_Ctrl5, (0x01 << 3) | (0x01 << 1))) {
        goto failed;
    }

    //使能陀螺仪、加速度计
    if (!_register_write(qmi8610, Qmi8610Register_Ctrl7, QMI8610_CONFIG_ACCGYR_ENABLE)) {
        goto failed;
    }

    devmgr_set_speed(qmi8610->_dev, DEV_SPEED_HIGH);
    devmgr_set_device_type(qmi8610->_dev, DEVTYPE_INS_QMI8610);

    console_printf("QMI8610 found on bus 0x%x\n", (unsigned)devmgr_get_bus_id(qmi8610->_dev));
    devmgr_release_bus(qmi8610->_dev);

    rt_thread_mdelay(10);

    return true;

failed:
    devmgr_release_bus(qmi8610->_dev);
    return false;
}

static uint8_t _register_read(sensor_imu_qmi8610_t qmi8610, uint8_t reg)
{
    uint8_t val = 0;

    devmgr_read_registers(qmi8610->_dev, reg, &val, 1);

    return val;
}

static inline bool _block_read(sensor_imu_qmi8610_t qmi8610, uint8_t reg, uint8_t *buf, uint32_t size)
{
    if (devmgr_get_bus_type(qmi8610->_dev) == BUS_TYPE_SPI) {
        reg |= QMI8610_SPI_AUTO_INC_ADD;
    } else {
        reg |= QMI8610_I2C_AUTO_INC_ADD;
    }

    return devmgr_read_registers(qmi8610->_dev, reg, buf, size);
}

static inline bool _register_write(sensor_imu_qmi8610_t qmi8610, uint8_t reg, uint8_t val)
{
    return devmgr_write_register(qmi8610->_dev, reg, val, false);
}

/*
  check whoami for sensor type
 */
static bool check_whoami(sensor_imu_qmi8610_t qmi8610)
{
    uint8_t whoami = _register_read(qmi8610, Qmi8610Register_WhoAmI);

    if (whoami == QMI8610_WHOAMI_VAL) {
        return true;
    }

    console_printf("[QMI8610]: _check_whoami fail(0x%X-0x%X)\n", QMI8610_WHOAMI_VAL, whoami);

    // not a value WHOAMI result
    return false;
}

/*
  read qmi8610
 */
static void read_sample(void *parameter)
{
    sensor_imu_qmi8610_t qmi8610 = (sensor_imu_qmi8610_t)parameter;

    uint8_t status0;
    if (!devmgr_read_registers(qmi8610->_dev, Qmi8610Register_Status0, &status0, 1)) {
        sensor_imu_backend_inc_accel_error_count(qmi8610->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8610->_gyro_instance);
        return;
    }

    // 检查加速度计、陀螺仪是否有新数据
    if (!(status0 & QMI8610STATUS0_ADA) || !(status0 & QMI8610STATUS0_GDA)) {
        return;
    }

    uint8_t data[sizeof(uint16_t) * 6];
    if (!_block_read(qmi8610, Qmi8610Register_Ax_L, (uint8_t *)&data[0], sizeof(data))) {
        sensor_imu_backend_inc_accel_error_count(qmi8610->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8610->_gyro_instance);
        return;
    }

    Vector3f_t accel = {(int16_t)((uint16_t)(data[0] | (data[1]<<8))) * qmi8610->_accel_scale,
                        (int16_t)((uint16_t)(data[2] | (data[3]<<8))) * qmi8610->_accel_scale,
                        (int16_t)((uint16_t)(data[4] | (data[5]<<8))) * qmi8610->_accel_scale};

    Vector3f_t gyro = {(int16_t)((uint16_t)(data[6] | (data[7]<<8))) * qmi8610->_gyro_scale,
                       (int16_t)((uint16_t)(data[8] | (data[9]<<8))) * qmi8610->_gyro_scale,
                       (int16_t)((uint16_t)(data[10] | (data[11]<<8))) * qmi8610->_gyro_scale};

    sensor_imu_backend_rotate_and_correct_accel(&qmi8610->backend, qmi8610->_accel_instance, &accel);
    sensor_imu_backend_notify_new_accel_raw_sample(&qmi8610->backend, qmi8610->_accel_instance, &accel, 0, false);

    sensor_imu_backend_rotate_and_correct_gyro(&qmi8610->backend, qmi8610->_gyro_instance, &gyro);
    sensor_imu_backend_notify_new_gyro_raw_sample(&qmi8610->backend, qmi8610->_gyro_instance, &gyro, 0);

    if (qmi8610->_temperature_counter++ == 100) {
        qmi8610->_temperature_counter = 0;
        int8_t t;
        if (!devmgr_read_registers(qmi8610->_dev, Qmi8610Register_Temperature, (uint8_t *)&t, sizeof(t))) {
            sensor_imu_backend_inc_accel_error_count(qmi8610->_accel_instance);
            sensor_imu_backend_inc_gyro_error_count(qmi8610->_gyro_instance);
        } else {
            float temp_degc = t;
            sensor_imu_backend_publish_temperature(&qmi8610->backend, qmi8610->_accel_instance, temp_degc);
        }
    }

    struct checkreg reg;
    if (!devmgr_check_next_register2(qmi8610->_dev, &reg)) {
        //log_register_change(dev_accel->get_bus_id(), reg);
        sensor_imu_backend_inc_accel_error_count(qmi8610->_accel_instance);
        sensor_imu_backend_inc_gyro_error_count(qmi8610->_gyro_instance);
    }
}


/*------------------------------------test------------------------------------*/


